# -*- coding: utf-8 -*-
import collections
import glob
import math
import os
import sys
import time
import weakref
import pyproj
import carla
import rospy
from datetime import datetime, timedelta
from v2x_msgs.msg import SPAT, IntersectionState, Phase, PhaseState

# Connect to client
client = carla.Client('192.168.3.157', 2000)
client.set_timeout(2.0)

world = client.get_world()
current_map = world.get_map()
actors = world.get_actors()

# 将xy坐标转换为gps坐标
geod = pyproj.Geod(ellps = "WGS84")     
def xy2gps(x,y,elevation):
    dis = math.sqrt(math.pow(x,2)+math.pow(y,2))
    azm = math.degrees(math.atan2(-y,x))
    #设置（125.1550768,43.8334643）为初始点坐标
    lon, lat, re_back_azm = geod.fwd(125.1550768,43.8334643,azm,dis)
    return lon, lat, elevation

def get_gps(actor):
    t = actor.get_transform()
    return xy2gps(t.location.x,t.location.y,t.location.z)


def talker():
    pub = rospy.Publisher('traffic', SPAT, queue_size=10)
    rospy.init_node('traffic', anonymous=True)
    rate = rospy.Rate(1) # 1hz
    while not rospy.is_shutdown():
        
        spat = SPAT()
        cr = Crossroad(world,2,[96,97,98])
        spat.intersections = cr.intersectionState
        pub.publish(spat)

        spat = SPAT()
        cr = Crossroad(world,3,[99,100,101])
        spat.intersections = cr.intersectionState
        pub.publish(spat)

        spat = SPAT()
        cr = Crossroad(world,5,[90,91,92])
        spat.intersections = cr.intersectionState
        pub.publish(spat)

        spat = SPAT()
        cr = Crossroad(world,6,[87,88,89])
        spat.intersections = cr.intersectionState
        pub.publish(spat)

        spat = SPAT()
        cr = Crossroad(world,7,[93,94,95])
        spat.intersections = cr.intersectionState
        pub.publish(spat)

        spat = SPAT()
        cr = Crossroad(world,8,[104,105,106])
        spat.intersections = cr.intersectionState
        pub.publish(spat)

        spat = SPAT()
        cr = Crossroad(world,9,[107,108,109])
        spat.intersections = cr.intersectionState
        pub.publish(spat)

        spat = SPAT()
        cr = Crossroad(world,11,[102,103,110])
        spat.intersections = cr.intersectionState
        pub.publish(spat)

        rospy.loginfo("published!")
        rate.sleep()

class Crossroad(object):
    def __init__(self, world, id, lights):
        self.world = world
        self.id = id
        self.lights = lights 
        self.intersectionState = []
        for light_id in lights:
            # print(light_id)
            light = world.get_actor(light_id)
            state = IntersectionState()
            state.nodeID = id
            state.phases = []
            phase = Phase()
            phase.id = light_id
            phaseStates = []
            lightState = light.get_state()

            #red
            if(str(lightState) == "Red"):
                red = PhaseState()
                red.LightState = 3
                red.startTime = 0
                red.likelyEndTIme = 20
                phaseStates.append(red)

                green = PhaseState()
                green.LightState = 5
                green.startTime = 20
                green.likelyEndTIme = 10
                phaseStates.append(green)

                yellow = PhaseState()
                yellow.LightState = 7
                yellow.startTime = 30
                yellow.likelyEndTIme = 3
                phaseStates.append(yellow)

            elif(str(lightState) == "Green"):
                red = PhaseState()
                red.LightState = 3
                red.startTime = 13 - light.get_elapsed_time()
                red.likelyEndTIme = 30
                phaseStates.append(red)

                green = PhaseState()
                green.LightState = 5
                green.startTime = 0
                green.likelyEndTIme = 10 - light.get_elapsed_time()
                phaseStates.append(green)

                yellow = PhaseState()
                yellow.LightState = 7
                yellow.startTime = 10 - light.get_elapsed_time()
                yellow.likelyEndTIme = 3
                phaseStates.append(yellow)

            elif(str(lightState) == "Yellow"):
                red = PhaseState()
                red.LightState = 3
                red.startTime = 3 - light.get_elapsed_time()
                red.likelyEndTIme = 30
                phaseStates.append(red)

                green = PhaseState()
                green.LightState = 5
                green.startTime = 0
                green.likelyEndTIme = 33 - light.get_elapsed_time()
                phaseStates.append(green)

                yellow = PhaseState()
                yellow.LightState = 7
                yellow.startTime = 0
                yellow.likelyEndTIme = 3
                phaseStates.append(yellow)

            phase.phaseStates = phaseStates
            state.phases.append(phase)
            self.intersectionState.append(state)
    
def main():
    talker()

if __name__ == '__main__':
    main()

